// Drone.cpp : Defines the entry point for the application.
//

#include "stdafx.h"
#include "Drone.h"

#define MAX_LOADSTRING 100
#define MY_LISTBOX 101
#define MY_SLIDER_PITCH 102
#define MY_STATIC_BOTTOM 103
#define MY_STATIC_TOP 104

//AT defines
#define DRONE_IP "192.168.1.1"
#define AT_PORT 5556

// Global Variables:
HINSTANCE hInst;								// current instance
TCHAR szTitle[MAX_LOADSTRING];					// The title bar text
TCHAR szWindowClass[MAX_LOADSTRING];			// the main window class name
HWND hWndMain;
WSADATA wsaData;
SOCKET sockATCMD=INVALID_SOCKET;
HBITMAP g_hbmDrone = NULL;


int iSeq=1;
bool isDroneStarted=false;
char szSendBuffer[MAX_PATH];
float fSpeed=(float)0.2;

// Forward declarations of functions included in this code module:
ATOM				MyRegisterClass(HINSTANCE hInstance);
BOOL				InitInstance(HINSTANCE, int);
LRESULT CALLBACK	WndProc(HWND, UINT, WPARAM, LPARAM);
INT_PTR CALLBACK	About(HWND, UINT, WPARAM, LPARAM);
bool SendATCmd(char * szCommand,int iLen);
bool SetUpATSocket();
void WriteToListBox(char * szMessage);
void MessageHandler(WPARAM key);
bool SendPCMD_AT(int enable,float pitch,float roll,float gaz,float yaw);

int APIENTRY _tWinMain(HINSTANCE hInstance,
                     HINSTANCE hPrevInstance,
                     LPTSTR    lpCmdLine,
                     int       nCmdShow)
{
	UNREFERENCED_PARAMETER(hPrevInstance);
	UNREFERENCED_PARAMETER(lpCmdLine);

 	// TODO: Place code here.
	MSG msg;
	HACCEL hAccelTable;

	INITCOMMONCONTROLSEX list;
	list.dwICC=ICC_LISTVIEW_CLASSES  |  //  list view
			   ICC_PROGRESS_CLASS    |  //  progress bar
    		   ICC_TREEVIEW_CLASSES  |  //  tree view & tooltip
    		   ICC_BAR_CLASSES       |  //  toolbar, status bar, trackbar & tooltip
    		   ICC_UPDOWN_CLASS      |  //  up-down
    		   ICC_USEREX_CLASSES    |  //  extended combo box
    		   ICC_TAB_CLASSES       |  //  tab & tooltip
    		   ICC_COOL_CLASSES      |  //  rebar
    		   ICC_DATE_CLASSES      ;  //  date & time picker
	list.dwSize=sizeof(list);
	InitCommonControlsEx(&list);
	// Initialize global strings
	LoadString(hInstance, IDS_APP_TITLE, szTitle, MAX_LOADSTRING);
	LoadString(hInstance, IDC_DRONE, szWindowClass, MAX_LOADSTRING);
	MyRegisterClass(hInstance);

	// Perform application initialization:
	if (!InitInstance (hInstance, nCmdShow))
	{
		return FALSE;
	}
	WriteToListBox("Press Space Bar to Take Off and land");
	WriteToListBox("Use Slider to change Speed");
	WriteToListBox("Use WSAD for up/down turn left/right");
	WriteToListBox("Use Cursor Block for move");
	
	hAccelTable = LoadAccelerators(hInstance, MAKEINTRESOURCE(IDC_DRONE));

	// Main message loop:
	while (GetMessage(&msg, NULL, 0, 0))
	{
		if (msg.message == WM_KEYDOWN)       
			MessageHandler(msg.wParam);
		if (!TranslateAccelerator(msg.hwnd, hAccelTable, &msg))
		{
			TranslateMessage(&msg);
			DispatchMessage(&msg);
		}
	}

	return (int) msg.wParam;
}

//write to list box
void WriteToListBox(char * szMessage)
{
	SendMessage(GetDlgItem(hWndMain,MY_LISTBOX), LB_ADDSTRING, 0, (LPARAM) szMessage);
	SendMessage(GetDlgItem(hWndMain,MY_LISTBOX), WM_VSCROLL, SB_BOTTOM, 0);
}

//some main functions for at handling
void MessageHandler(WPARAM wParam)
{
	float pitch=0;
	float roll=0;
	float gaz=0;
	float yaw=0;

	switch(wParam)
	{
	case 37: //move left roll- Cursor left
		{
			roll=-fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case 38: //move forward pitch+ Cursor up
		{
			pitch=fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case 39: //move right roll+ Cursor right
		{
			roll=fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case 40: //move backward pitch- Cursor down
		{
			pitch=-fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case VK_SPACE:
		if (!isDroneStarted)
		{
			SendMessage(GetDlgItem(hWndMain,MY_LISTBOX),LB_RESETCONTENT,0,0); 
			//send an trim and an start command
			SendATCmd("AT*CONFIG=\"general:navdata_demo\",\"FALSE\"\r",strlen("AT*CONFIG=\"general:navdata_demo\",\"TRUE\"\r"));
			//SendATCmd("AT*CTRL=5,0\r",strlen("AT*CTRL=5,0\r"));
			iSeq=1;
			sprintf_s(szSendBuffer,"AT*CONFIG=%d,\"control:altitude_max\",\"2000\"\r",iSeq);
			SendATCmd(szSendBuffer,strlen(szSendBuffer));
			sprintf_s(szSendBuffer,"AT*SEQ=%d\r",++iSeq);
			SendATCmd(szSendBuffer,strlen(szSendBuffer));
			SendATCmd("AT*FTRIM\r",strlen("AT*FTRIM\r"));
			sprintf_s(szSendBuffer,"AT*REF=%d,290718208\r",++iSeq);
			SendATCmd(szSendBuffer,strlen(szSendBuffer));
			isDroneStarted=true;
			SetTimer(hWndMain,200,1000,(TIMERPROC)NULL); //poll hovering every 2 second
		}
		else
		{
			KillTimer(hWndMain,200);
			sprintf_s(szSendBuffer,"AT*REF=%d,290717696\r",++iSeq);
			SendATCmd(szSendBuffer,strlen(szSendBuffer));
			isDroneStarted=false;
		}
		break;
	case 65: //rotate left yaw- KEY A
		{
			yaw=-fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case 68: //rotate right yaw+ KEY D
		{
			yaw=fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case 83: //go down gaz- KEY S
		{
			gaz=-fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case 87: //go up gaz+ KEY W
		{
			gaz=fSpeed;
			SendPCMD_AT(1,pitch,roll,gaz,yaw);
		}
		break;
	case 82: //Reset Watchdog
		{
			sprintf_s(szSendBuffer,"AT*COMWDG=%d\r",++iSeq);
			SendATCmd(szSendBuffer,strlen(szSendBuffer));
		}
		break;
	}
	return;
}

bool SetUpATSocket()
{
    struct hostent *localHost;
    char *ip;
	struct sockaddr_in LocalAddr;
    int LocalAddrSize = sizeof (LocalAddr);
	int rc;

	sockATCMD =	WSASocket(AF_INET, SOCK_DGRAM, IPPROTO_UDP, NULL, 0,WSA_FLAG_OVERLAPPED);
    if (sockATCMD == INVALID_SOCKET)
	{
		sprintf_s(szSendBuffer,"socket failed: %d\n", WSAGetLastError());
		WriteToListBox(szSendBuffer);
        return false;
    }
    localHost = gethostbyname("");
    ip = inet_ntoa(*(struct in_addr *) *localHost->h_addr_list);

    LocalAddr.sin_family = AF_INET;
    LocalAddr.sin_addr.s_addr = INADDR_ANY;//inet_addr(ip);
    LocalAddr.sin_port = htons(AT_PORT+100);

    //---------------------------------------------
    rc = bind(sockATCMD, (struct sockaddr *) &LocalAddr, LocalAddrSize);
    if (rc == SOCKET_ERROR)
	{
        return false;
    }

	return true;
}

/*bool SetUpATSocket()
{
	if(sockATCMD == INVALID_SOCKET ) 
	{
        struct sockaddr_in at_udp_addr;

        memset( (char*)&at_udp_addr, 0, sizeof(at_udp_addr) );

        at_udp_addr.sin_family      = AF_INET;
        at_udp_addr.sin_addr.s_addr = INADDR_ANY;
        at_udp_addr.sin_port        = htons( AT_PORT +100 );

        sockATCMD = socket( AF_INET, SOCK_DGRAM, 0 );
    
        if( sockATCMD != INVALID_SOCKET )
		{
			u_long iMode = 1; //1 non blocking mode 0 would be blocking mode
			if(ioctlsocket(sockATCMD, FIONBIO, &iMode)!=0)
			{
				WriteToListBox("Set socket to non blocking failed");
				return false;
			}
			if (bind(sockATCMD, (struct sockaddr*)&at_udp_addr, sizeof(struct sockaddr)) < 0) 
			{
				WriteToListBox("bind socket failed");
				return false;
			}
		}
    }	
	return true;
}*/


bool SendATCmd(char* szCommand,int iLen)
{
    WSABUF DataBuf;

	WSAOVERLAPPED Overlapped = { 0 };
    struct sockaddr_in RecvAddr;
    int RecvAddrSize = sizeof (RecvAddr);

    
    DWORD BytesSent = 0;
    DWORD Flags = 0;

    int rc, err;
    int retval = 0;


    RecvAddr.sin_family = AF_INET;

    RecvAddr.sin_addr.s_addr = inet_addr(DRONE_IP);
    if (RecvAddr.sin_addr.s_addr == INADDR_NONE)  
	{
        return false;
    }
    RecvAddr.sin_port = htons(AT_PORT);

	if(RecvAddr.sin_port == 0)
	{
        return false;
    }


    Overlapped.hEvent = WSACreateEvent();
    if (Overlapped.hEvent == NULL) 
	{
		sprintf_s(szSendBuffer,"WSACreateEvent failed: %d\n", WSAGetLastError());
		WriteToListBox(szSendBuffer);
        return false;
    }

    DataBuf.len = iLen;
    DataBuf.buf = szCommand;
    rc = WSASendTo(sockATCMD, &DataBuf, 1,&BytesSent, Flags, (SOCKADDR *) & RecvAddr,RecvAddrSize, &Overlapped, NULL);

    if ((rc == SOCKET_ERROR) && (WSA_IO_PENDING != (err = WSAGetLastError()))) 
	{
        return false;
    }

    rc = WSAWaitForMultipleEvents(1, &Overlapped.hEvent, TRUE, INFINITE, TRUE);
    if (rc == WSA_WAIT_FAILED) 
	{
        return false;
    }

    rc = WSAGetOverlappedResult(sockATCMD, &Overlapped, &BytesSent,FALSE, &Flags);
    if (rc == FALSE) 
	{
        return false;
    }
	WSACloseEvent(Overlapped.hEvent);
	WriteToListBox(szCommand);
	Sleep(1);
	return true;
}


/*bool SendATCmd(char * szCommand,int iLen)
{
	struct sockaddr_in to;
    if( sockATCMD != INVALID_SOCKET )
	{
		int res;
        memset( (char*)&to, 0, sizeof(to) );
        to.sin_family       = AF_INET;
        to.sin_addr.s_addr  = inet_addr(DRONE_IP); // BROADCAST address for subnet 192.168.1.xxx
        to.sin_port         = htons (AT_PORT);

        res = sendto( sockATCMD, szCommand, iLen, 0, (struct sockaddr*)&to, sizeof(to) );
		if (res<0)
			return false;

		WriteToListBox(szCommand);
		Sleep (1);
		return true;
	}
	else
		return false;


}*/

bool SendPCMD_AT(int enable,float pitch,float roll,float gaz,float yaw)
{
	FLOATORINT fiPitch,fiRoll,fiGaz,fiYaw;
	fiPitch.f=pitch;
	fiRoll.f=roll;
	fiGaz.f=gaz;
	fiYaw.f=yaw;
	
	sprintf_s(szSendBuffer,"AT*PCMD=%d,%d,%d,%d,%d,%d\r",++iSeq,enable,fiPitch.i,fiRoll.i,fiGaz.i,fiYaw.i);
	SendATCmd(szSendBuffer,strlen(szSendBuffer));
	return true;
}

//
//  FUNCTION: MyRegisterClass()
//
//  PURPOSE: Registers the window class.
//
//  COMMENTS:
//
//    This function and its usage are only necessary if you want this code
//    to be compatible with Win32 systems prior to the 'RegisterClassEx'
//    function that was added to Windows 95. It is important to call this function
//    so that the application will get 'well formed' small icons associated
//    with it.
//
ATOM MyRegisterClass(HINSTANCE hInstance)
{
	WNDCLASSEX wcex;

	wcex.cbSize = sizeof(WNDCLASSEX);

	wcex.style			= CS_HREDRAW | CS_VREDRAW;
	wcex.lpfnWndProc	= WndProc;
	wcex.cbClsExtra		= 0;
	wcex.cbWndExtra		= 0;
	wcex.hInstance		= hInstance;
	wcex.hIcon			= LoadIcon(hInstance, MAKEINTRESOURCE(IDI_DRONE));
	wcex.hCursor		= LoadCursor(NULL, IDC_ARROW);
	wcex.hbrBackground	= (HBRUSH)(COLOR_BTNFACE + 1);
	wcex.lpszMenuName	= MAKEINTRESOURCE(IDC_DRONE);
	wcex.lpszClassName	= szWindowClass;
	wcex.hIconSm		= LoadIcon(wcex.hInstance, MAKEINTRESOURCE(IDI_SMALL));

	return RegisterClassEx(&wcex);
}

//
//   FUNCTION: InitInstance(HINSTANCE, int)
//
//   PURPOSE: Saves instance handle and creates main window
//
//   COMMENTS:
//
//        In this function, we save the instance handle in a global variable and
//        create and display the main program window.
//
BOOL InitInstance(HINSTANCE hInstance, int nCmdShow)
{
   hInst = hInstance; // Store instance handle in our global variable

   hWndMain = CreateWindow(szWindowClass, szTitle, WS_OVERLAPPEDWINDOW,
      10, 10, 640,480, NULL, NULL, hInstance, NULL);

   if (!hWndMain)
   {
      return FALSE;
   }

   ShowWindow(hWndMain, nCmdShow);
   UpdateWindow(hWndMain);

   return TRUE;
}

//
//  FUNCTION: WndProc(HWND, UINT, WPARAM, LPARAM)
//
//  PURPOSE:  Processes messages for the main window.
//
//  WM_COMMAND	- process the application menu
//  WM_PAINT	- Paint the main window
//  WM_DESTROY	- post a quit message and return
//
//
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
{
	HWND hWndOwner;
	RECT rcOwner,rcDlg,rc;
	int wmId, wmEvent;
	HDC hdc;
	float pitch=0;
	float roll=0;
	float gaz=0;
	float yaw=0;
	
	switch (message)
	{
	case WM_CREATE:
		{
			//middle position
			if ((hWndOwner = GetParent (hWnd)) == NULL)
			{
				hWndOwner = GetDesktopWindow (); 
			}
			GetWindowRect (hWndOwner, &rcOwner); 
			GetWindowRect (hWnd, &rcDlg); 
			CopyRect (&rc, &rcOwner); 
			OffsetRect (&rcDlg, -rcDlg.left, -rcDlg.top); 
			OffsetRect (&rc, -rc.left, -rc.top); 
			OffsetRect (&rc, -rcDlg.right, -rcDlg.bottom); 
			// The new position is the sum of half the remaining 
			// space and the owner's original position. 
			SetWindowPos (hWnd, HWND_TOP, rcOwner.left + (rc.right / 2), rcOwner.top + (rc.bottom / 2), 0, 0, SWP_NOSIZE);

			//just a debug window for output
			HWND hChild;
			hChild = CreateWindowEx (WS_EX_CLIENTEDGE, _T("LISTBOX"), NULL,
									 WS_CHILD | WS_VISIBLE |WS_VSCROLL|WS_HSCROLL,
									 0, 0, 0, 0, hWnd,
									 (HMENU)MY_LISTBOX, hInst, NULL);

			hChild = CreateWindowEx (WS_EX_CLIENTEDGE, TRACKBAR_CLASS,"Speed",
									WS_CHILD | WS_VISIBLE | TBS_AUTOTICKS|TBS_ENABLESELRANGE|TBS_VERT,
									0, 0, 0, 0,
									hWnd, (HMENU)MY_SLIDER_PITCH, hInst, NULL);

			SendMessage(hChild, TBM_SETRANGE, (WPARAM) TRUE, (LPARAM) MAKELONG(1, 10)); 
			SendMessage(hChild, TBM_SETPAGESIZE, 0, (LPARAM) 4); 
			SendMessage(hChild, TBM_SETSEL, (WPARAM) FALSE,(LPARAM) MAKELONG(1, 10)); 
			SendMessage(hChild, TBM_SETPOS, (WPARAM) TRUE, (LPARAM) 10); 

			/*hChild = CreateWindowEx(0, "STATIC", "1", 
									SS_CENTER | WS_CHILD | WS_VISIBLE,
									0, 0, 30, 20, 
									hWnd, (HMENU)MY_STATIC_TOP, hInst, NULL);
			SendMessage(GetDlgItem(hWnd,MY_SLIDER_PITCH), TBM_SETBUDDY,(WPARAM)TRUE, (LPARAM)hChild);

			hChild = CreateWindowEx(0, "STATIC", "0.1", 
									SS_CENTER | WS_CHILD | WS_VISIBLE,
									0, 0, 30, 20, 
									hWnd, (HMENU)MY_STATIC_BOTTOM, hInst, NULL);
			SendMessage(GetDlgItem(hWnd,MY_SLIDER_PITCH), TBM_SETBUDDY,(WPARAM)FALSE, (LPARAM)hChild);*/
			g_hbmDrone=(HBITMAP)LoadImage(GetModuleHandle(NULL),MAKEINTRESOURCE(IDB_BITMAP1), IMAGE_BITMAP, 0,0, LR_CREATEDIBSECTION);

 
		    if(WSAStartup(MAKEWORD(2,0),&wsaData) != 0)
			{
				PostQuitMessage(WSAGetLastError());
			}
			if (!SetUpATSocket())
			{
				PostQuitMessage(WSAGetLastError());
			}
		}
		break;
	case WM_TIMER:
		switch (wParam)
		{
			case 200:
				SendPCMD_AT(0,pitch,roll,gaz,yaw);
				break;
		}
		break;
	case WM_SIZE:
	case WM_WINDOWPOSCHANGING:
	case WM_WINDOWPOSCHANGED:
		{
			HDWP hDWP;
			RECT rc;
			if(hDWP = BeginDeferWindowPos(2))
			{ 
				GetClientRect(hWnd, &rc);
				hDWP = DeferWindowPos(hDWP, GetDlgItem(hWnd, MY_LISTBOX), NULL,
					rc.left+2, rc.top+5, rc.right -40 , rc.bottom - 150,
					SWP_NOZORDER | SWP_NOREDRAW);
				hDWP = DeferWindowPos(hDWP, GetDlgItem(hWnd, MY_SLIDER_PITCH), NULL,
					rc.right-35,rc.top + 5,30,rc.bottom - 150,
					SWP_NOZORDER | SWP_NOREDRAW);
				EndDeferWindowPos(hDWP);
				RedrawWindow(hWnd, NULL, NULL, RDW_INVALIDATE | RDW_ALLCHILDREN | RDW_NOERASE | RDW_UPDATENOW);
			}
			//SendMessage(GetDlgItem(hWnd,MY_SLIDER_PITCH), TBM_SETBUDDY,(WPARAM)FALSE, (LPARAM)GetDlgItem(hWnd,MY_STATIC_BOTTOM));
			//SendMessage(GetDlgItem(hWnd,MY_SLIDER_PITCH), TBM_SETBUDDY,(WPARAM)TRUE, (LPARAM)GetDlgItem(hWnd,MY_STATIC_TOP));
		}
		return 0;
	case WM_VSCROLL :
		switch(LOWORD(wParam))
		{
		case TB_THUMBTRACK : 
			DWORD dwPos = 11-SendMessage(GetDlgItem(hWnd,MY_SLIDER_PITCH), TBM_GETPOS, 0, 0);
			fSpeed=(float)dwPos/(float)10;
			sprintf_s(szSendBuffer,"Speed now: %.1f",fSpeed);
			SetFocus(hWnd);
			WriteToListBox(szSendBuffer);
			break; 
		}
		break;
	case WM_COMMAND:
		wmId    = LOWORD(wParam);
		wmEvent = HIWORD(wParam);
		// Parse the menu selections:
		switch (wmId)
		{
		case IDM_ABOUT:
			DialogBox(hInst, MAKEINTRESOURCE(IDD_ABOUTBOX), hWnd, About);
			break;
		case IDM_EXIT:
			DestroyWindow(hWnd);
			break;
		default:
			return DefWindowProc(hWnd, message, wParam, lParam);
		}
		break;
	case WM_PAINT:
		{
			BITMAP bm;
			PAINTSTRUCT ps;
			GetClientRect(hWnd, &rc);
			hdc = BeginPaint(hWnd, &ps);
	        HDC hdcMem = CreateCompatibleDC(hdc);
	        HBITMAP hbmOld = (HBITMAP)SelectObject(hdcMem, g_hbmDrone);
			GetObject(g_hbmDrone, sizeof(bm), &bm);
			SetStretchBltMode(hdc,COLORONCOLOR);
			StretchBlt(hdc,rc.left,rc.bottom-150,rc.right,150,hdcMem,0,0,bm.bmWidth, bm.bmHeight,SRCCOPY);
			SelectObject(hdcMem, hbmOld);
			DeleteDC(hdcMem);
			EndPaint(hWnd, &ps);
		}
		break;
	case WM_DESTROY:
		if (sockATCMD!=INVALID_SOCKET)
			closesocket(sockATCMD);
		WSACleanup();
		PostQuitMessage(0);
		break;
	default:
		return DefWindowProc(hWnd, message, wParam, lParam);
	}
	return 0;
}

// Message handler for about box.
INT_PTR CALLBACK About(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
{
	UNREFERENCED_PARAMETER(lParam);
	switch (message)
	{
	case WM_INITDIALOG:
		return (INT_PTR)TRUE;

	case WM_COMMAND:
		if (LOWORD(wParam) == IDOK || LOWORD(wParam) == IDCANCEL)
		{
			EndDialog(hDlg, LOWORD(wParam));
			return (INT_PTR)TRUE;
		}
		break;
	}
	return (INT_PTR)FALSE;
}
